/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Victor;
import java.util.TimerTask;

/**
 *
 * @author Daedalus
 */
public class ShooterControl  extends TimerTask 
{
    private Victor shooterMtr;
    
    private Timer warmupTimer;
    private Timer cylinderTimer;
    private Timer shotTimer;
    
    private Solenoid trapDoor;
    
    Joystick driverJoy;
    Joystick shooterJoy;
    
    private double DEADZONE = 0.5;

    private double WARMUP_DELAY = 500.0;   // milliseconds
    private double CYLINDER_DELAY = 250.0; //milliseconds
    private double SHOT_DELAY = 250.0;   // milliseconds
    
    private boolean readyToShoot = false;
    private boolean goFast = true;
    
    private int currentState;
    /*
     * State constants
     */
    private final int ST_IDLE = 1;
    private final int ST_CONVEYOR_STARTING = 2; // Motor warming up
    private final int ST_SHOOTER_READY = 3;
    private final int ST_SHOOTER_IN_MOTION = 4;
    private final int ST_BETWEEN_SHOTS = 5;
    private final int ST_WAIT_FOR_TRIGGER_RELEASE = 6;
    
    private final boolean ON = true;
    private final boolean OFF = false;
    
    private boolean shooterPositionHigh;
    private final boolean HIGH_SHOOTER = true;
    private final boolean LOW_SHOOTER = false;
    
    private final boolean CYL_EXTENDED = true;
    private final boolean CYL_RETRACTED = true;
    
    private boolean triggerEnabled = false;
    private double INTERVAl_CYLINDER = 200.0;
    private double INTERVAl_WARMUP = 200.0;
    
    /*
     * External action constants
     */
    private final int INPUT_JOYSTICK_ACTIVE = 1;
    private final int INPUT_WARMUP_TIMER_EXPIRES = 2;
    private final int INPUT_TRIGGER_PRESSED = 3;
    private final int INPUT_CYLINDER_TIMER_EXPIRES = 4;
    private final int INPUT_BETWEEN_SHOT_TIMER_EXPIRES = 5;
    private final int INPUT_TRIGGER_RELEASED = 6;
    private final int INPUT_JOYSTICK_IDLE = 7;
    
    /*
     * Robot steps constants
     */
    private final int STEP_START_WARMUP_TIMER = 1;
    private final int STEP_START_BETWEEN_SHOT_TIMER = 2;
    private final int STEP_START_CYLINDER_SHOT_TIMER = 3;
    private final int STEP_START_SHOOTER_MOTOR = 4;
    private final int STEP_ENABLE_TRIGGER = 5;
    private final int STEP_DISABLE_TRIGGER = 6;
    private final int STEP_STOP_SHOOTER_MOTOR = 7;
    private final int STEP_EXTEND_SHOOTER_CYLINDER = 8;
    private final int STEP_RETRACT_SHOOTER_CYLINDER = 9;
    
    /*
     * Robot actions constants
     */
    private final int ACTION_START_SHOOTER = 1;
    private final int ACTION_ENABLE_TRIGGER = 2;
    private final int ACTION_SHOOT_BALL = 3;
    private final int ACTION_RETRACT_CYLINDER = 4;
    private final int ACTION_STOP_SHOOTER = 5;
    
    private double SHOOTER_MTR_SPEED = 1.0;

    
    
    private ControlStep[] controlSequencer; 
    
    public ShooterControl()
    {
        shooterMtr = new Victor(Constants.SHOOTERMTR_SLOT);
        shooterJoy = new Joystick(Constants.SHOOTER_JOY);
        trapDoor = new Solenoid(Constants.TRAP_DOOR);
        warmupTimer = new Timer();
        warmupTimer.reset();
        createControlSequence();
    }
    
    private void createControlSequence()
    {
        controlSequencer = new ControlStep[7];
        controlSequencer[0] = new ControlStep(ST_IDLE,INPUT_JOYSTICK_ACTIVE,ACTION_START_SHOOTER,ST_CONVEYOR_STARTING);
        controlSequencer[1] = new ControlStep(ST_CONVEYOR_STARTING,INPUT_WARMUP_TIMER_EXPIRES,ACTION_ENABLE_TRIGGER,ST_SHOOTER_READY);
        controlSequencer[2] = new ControlStep(ST_SHOOTER_READY,INPUT_TRIGGER_PRESSED,ACTION_SHOOT_BALL,ST_SHOOTER_IN_MOTION);
        controlSequencer[3] = new ControlStep(ST_SHOOTER_IN_MOTION,INPUT_CYLINDER_TIMER_EXPIRES,ACTION_RETRACT_CYLINDER,ST_BETWEEN_SHOTS);
        controlSequencer[4] = new ControlStep(ST_BETWEEN_SHOTS,INPUT_BETWEEN_SHOT_TIMER_EXPIRES,ACTION_ENABLE_TRIGGER,ST_WAIT_FOR_TRIGGER_RELEASE);
        controlSequencer[5] = new ControlStep(ST_WAIT_FOR_TRIGGER_RELEASE, INPUT_JOYSTICK_IDLE, ACTION_STOP_SHOOTER, ST_IDLE);
        controlSequencer[6] = new ControlStep(ST_WAIT_FOR_TRIGGER_RELEASE,INPUT_TRIGGER_RELEASED,ACTION_ENABLE_TRIGGER,ST_SHOOTER_READY);
        } 
    
    /**
     * Checks specified external input based on inputToCheck variable
     * 
     * @param inputToCheck
     * @return boolean value given by appropriate input checker method
     */
    private boolean checkInput(int inputToCheck){
        switch(inputToCheck)
        {
            case INPUT_JOYSTICK_ACTIVE:
                return input_isJoyStickActive();
            case INPUT_WARMUP_TIMER_EXPIRES:
                return input_isWarmUpTimerExpires();
            case INPUT_TRIGGER_PRESSED:
                return input_isTriggerPushed();
            case INPUT_CYLINDER_TIMER_EXPIRES:
                return input_isCylinderTimerExpires();
            case INPUT_BETWEEN_SHOT_TIMER_EXPIRES:
                return input_isBetweenShotTimerExpires();
            case INPUT_TRIGGER_RELEASED:
                return input_isTriggerReleased();
            case INPUT_JOYSTICK_IDLE:
                return input_isJoystickIdle();
        }
        return false;
    }
    
    /**
     * Execute appropriate robot action function based on given integer
     * @param robotStep Checks this integer against the robot action constants
     */
    private void executeStep(int robotStep){
        switch(robotStep){
            case STEP_START_WARMUP_TIMER:
                startWarmupTimer();
                return;
            case STEP_START_BETWEEN_SHOT_TIMER:
                startShotTimer();
                return;
            case STEP_START_CYLINDER_SHOT_TIMER:
                startCylinderTimer();
                return;
            case STEP_START_SHOOTER_MOTOR:
                startShooterMotor();
                return;
            case STEP_ENABLE_TRIGGER:
                enableTrigger();
                return;
            case STEP_DISABLE_TRIGGER:
                disableTrigger();
                return;
            case STEP_STOP_SHOOTER_MOTOR:
                stopShooterMotor();
                return;
            case STEP_EXTEND_SHOOTER_CYLINDER:
                extendShooterCylinder();
                return;
            case STEP_RETRACT_SHOOTER_CYLINDER:
                retractShooterCylinder();
                return;
        }
    }

    private void robotAction(int robotAction)
    {
        switch(robotAction)
        {
            case ACTION_START_SHOOTER:
                executeStep(STEP_START_SHOOTER_MOTOR);
                executeStep(STEP_START_WARMUP_TIMER);
                return;
            case ACTION_ENABLE_TRIGGER:
                executeStep(STEP_ENABLE_TRIGGER);
                return;
            case ACTION_SHOOT_BALL:
                executeStep(STEP_EXTEND_SHOOTER_CYLINDER);
                executeStep(STEP_START_CYLINDER_SHOT_TIMER);
                executeStep(STEP_DISABLE_TRIGGER);
                return;
            case ACTION_RETRACT_CYLINDER:
                executeStep(STEP_START_BETWEEN_SHOT_TIMER);
                executeStep(STEP_RETRACT_SHOOTER_CYLINDER);
                return;
            case ACTION_STOP_SHOOTER:
                executeStep(STEP_STOP_SHOOTER_MOTOR);
                executeStep(STEP_DISABLE_TRIGGER);
                return;
        }
    }
    /*
     * Child class representing each state configuration
     * @currentState    State this control step represents
     * @input           The input required to execute robotAction
     * @nextState       The next ControlStep that will be achieved by executing robotAction
     * @robotAction     The action to be executed
     */
    private class ControlStep
    {
        private int currentState;
        private int input;
        private int robotAction;
        private int nextState;
        
        ControlStep( int currentState, int input, int nextState, int robotAction)
        {
            this.currentState = currentState;
            this.input = input;
            this.nextState = nextState;
            this.robotAction = robotAction;
        }
        
        public int getCurrentState() {
            return currentState;
        }

        public int getInput() {
            return input;
        }

        public int getNextState() {
            return nextState;
        }

        public int getRobotAction() {
            return robotAction;
        }

    } 
    
    public void run()
    {
        ControlStep step;
              for(int i = 0; i < controlSequencer.length; i++)
              {
                 step = controlSequencer[i];
                 if (step.getCurrentState() == currentState)
                 {
                     if (checkInput(step.getInput()))
                     {
                         robotAction(step.getRobotAction());
                         currentState = step.getNextState();
                     }
                     
                 }
              }
                  
    }
    
    /*
     * External Action checker methods. These methods are in order so they reflect their respective constants
     * (i.e INPUT_JOYSTICK_ACTIVE constant equals 1, and input_isJoyStickActive() is the first method)
     */
   
    private boolean input_isJoyStickActive()
    {
        double y = shooterJoy.getY();        
        return (Math.abs(y) > DEADZONE);
    }

    private boolean input_isWarmUpTimerExpires() 
    {
        return warmupTimer.get() >= WARMUP_DELAY*1000; //convert from milliseconds to microseconds
    }

    private boolean input_isTriggerPushed() 
    {
        return shooterJoy.getRawButton(Constants.SHOOTER_BUTTON);
    }

    private boolean input_isCylinderTimerExpires() 
    {
        return cylinderTimer.get() >= CYLINDER_DELAY * 1000; //convert from milliseconds to microseconds
    }

    private boolean input_isBetweenShotTimerExpires() 
    {
        return shotTimer.get() >= SHOT_DELAY * 1000; //convert from milliseconds to microseconds
    }

    private boolean input_isTriggerReleased() 
    {
        return !shooterJoy.getRawButton(Constants.SHOOTER_BUTTON);
    }

    private boolean input_isJoystickIdle() 
    {
        return !input_isJoyStickActive();
    }
    
    /*
     * Robot action methods
     */
    public void startWarmupTimer(){
        warmupTimer.reset();
        warmupTimer.start();
    }
    
    public void startShotTimer(){
        shotTimer.reset();
        shotTimer.start();
    }
    
    public void startCylinderTimer(){
        cylinderTimer.reset();
        cylinderTimer.start();
    }
    
    public void startShooterMotor(){
        shooterMtr.set(SHOOTER_MTR_SPEED);
    }
    
    public void enableTrigger(){
        this.triggerEnabled = true;
    }
    
    public void disableTrigger(){
        this.triggerEnabled = true;
    }
    
    public void stopShooterMotor(){
        shooterMtr.set(0);
    }
    
    public void extendShooterCylinder(){
        
    }
    
    public void retractShooterCylinder(){
        
    }
}
